#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "std_msgs/String.h"
#include "std_msgs/Time.h"
#include <algorithm>
#include <memory>
using namespace std;
#include <sstream>
class FinalTimeExpire {
public:
  ros::Time t;
  ros::NodeHandle n;
  ros::Publisher time_pub;
  ros::Timer timer;
  string name;

  void timerCallback(const ros::TimerEvent &e) {
    ROS_INFO_STREAM(this<<"  in timerCallback");
    std_msgs::Time time;
    time.data = t;
    time_pub.publish(time);
    ros::spinOnce();
  };
  FinalTimeExpire(ros::NodeHandle n,string name) {
    this->n = n;
    this->name = name;
    time_pub = n.advertise<std_msgs::Time>(name+"/FinalTimeExpiretopic", 1);

    t = ros::Time::now();
    timer = n.createTimer(ros::Duration(10), &FinalTimeExpire::timerCallback, this,
                          true);
  }

  ~FinalTimeExpire() {
    // ROS_INFO_STREAM("  in ~FinalTimeExpire");
  }
};
typedef shared_ptr<FinalTimeExpire> FinalTimeExpirePtr;

class FinalTimeMgr {
public:
  // vector<pair<FinalTimeExpirePtr, ros::Subscriber>> arr;
  ros::NodeHandle n;
  string name;

  ros::Publisher count_pub;

  int iThreshold = 10;
  int iThresholdSelf = 10;
  vector<FinalTimeExpirePtr> arr;
  vector<ros::Subscriber> subArr;
  ros::Subscriber sub;


  FinalTimeMgr(ros::NodeHandle n,string name) {
    this->n = n;
    this->name = name;


    count_pub = n.advertise<std_msgs::Bool>(name+"/threshold", 10);
  }
  void msgCallback(const std_msgs::Bool::ConstPtr &msg) {
    ROS_INFO_STREAM(name+"  in msgCallback");
    FinalTimeExpirePtr te = make_shared<FinalTimeExpire>(n,name);
    // pair<FinalTimeExpirePtr, ros::Subscriber> p(
    //     te, n.subscribe("FinalTimeExpiretopic", 1000, &FinalTimeMgr::msgtimedelCallback,
    //                     this));
    // arr.push_back(pair<FinalTimeExpirePtr, ros::Subscriber>(
    //     te, n.subscribe("FinalTimeExpiretopic", 1000, &FinalTimeMgr::msgtimedelCallback,
    //                     this)));

    arr.push_back(te);
    sub=n.subscribe(name+"/FinalTimeExpiretopic", 1000, &FinalTimeMgr::msgtimedelCallback,
                        this);
    // subArr.push_back(n.subscribe("FinalTimeExpiretopic", 1000, &FinalTimeMgr::msgtimedelCallback,
    //                     this)) ;
    ROS_INFO_STREAM("arr size:" << arr.size());

    if (arr.size() > iThreshold) {
      std_msgs::Bool count_flag;
      count_flag.data = true;
      count_pub.publish(count_flag);
      ros::spinOnce();
    }
    // ros::Subscriber sub = n.subscribe("FinalTimeExpire", 1000,
    // &FinalTimeMgr::msgtimedelCallback, this);
    // subArr.push_back();
  }
  void msgtimedelCallback(const std_msgs::Time::ConstPtr &msg) {
    ROS_INFO_STREAM(name+"  in msgtimedelCallback");
    // arr.erase(std::remove_if(arr.begin(), arr.end(),
    //                             [&](pair<FinalTimeExpirePtr, ros::Subscriber> p) {
    //                               return p.first == nullptr ||
    //                                      p.first->t == msg->data;
    //                               // return true;
    //                             }));

    bool flag = false;
    auto iter = arr.begin();
    for (; iter != arr.end(); iter++) {
      assert(*iter != nullptr);
      // if (*iter == nullptr) {
      //   flag = true;
      //   ROS_INFO_STREAM("is null!!!!!!!!!!!!!!!!");
      //   break;
      // }
      if ((*iter)->t == msg->data) {
        flag = true;
        break;
      }
    }

    if (flag) {
      arr.erase(iter);
      ROS_INFO_STREAM(name+"  arr size:" << arr.size());

      if (arr.size() > iThreshold) {
        std_msgs::Bool count_flag;
        count_flag.data = true;
        count_pub.publish(count_flag);
        ros::spinOnce();
      }
    }
    else{
      ROS_INFO_STREAM(name+"uuuuuuuuuuuuuuuuuuuuu:" << arr.size());

    }
  }
};

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char *argv[])
{
	/**
	 * The ros::init() function needs to see argc and argv so that it can perform
	 * any ROS arguments and name remapping that were provided at the command line.
	 * For programmatic remappings you can use a different version of init() which takes
	 * remappings directly, but for most command-line programs, passing argc and argv is
	 * the easiest way to do it.  The third argument to init() is the name of the node.
	 *
	 * You must call one of the versions of ros::init() before using any other
	 * part of the ROS system.
	 */
	ros::init(argc, argv, "countinperiod_pub");

	/**
	 * NodeHandle is the main access point to communications with the ROS system.
	 * The first NodeHandle constructed will fully initialize this node, and the last
	 * NodeHandle destructed will close down the node.
	 */
	ros::NodeHandle n;

	/**
	 * The advertise() function is how you tell ROS that you want to
	 * publish on a given topic name. This invokes a call to the ROS
	 * master node, which keeps a registry of who is publishing and who
	 * is subscribing. After this advertise() call is made, the master
	 * node will notify anyone who is trying to subscribe to this topic name,
	 * and they will in turn negotiate a peer-to-peer connection with this
	 * node.  advertise() returns a Publisher object which allows you to
	 * publish messages on that topic through a call to publish().  Once
	 * all copies of the returned Publisher object are destroyed, the topic
	 * will be automatically unadvertised.
	 *
	 * The second parameter to advertise() is the size of the message queue
	 * used for publishing messages.  If messages are published more quickly
	 * than we can send them, the number here specifies how many messages to
	 * buffer up before throwing some away.
	 */
	 FinalTimeMgr t_self(n, "final");
	  ros::NodeHandle n1;
  ros::Subscriber sub_self =
      n1.subscribe("origin/threshold", 1000, &FinalTimeMgr::msgCallback, &t_self);

  ros::spin();

	return 0;
}